#!/usr/bin/env python
import roslib; roslib.load_manifest('robcar_ros')
import rospy

from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from robcar_ros.msg import move1D

vel = Twist()
odo = Odometry()
move = move1D()
pose = 0.0
int_pose = 0
target = 0.0
moving = False

#Velocity publisher:
def vel_pub():

	global vel
	global target
	global moving
	global move
	
	pub = rospy.Publisher('cmd_vel', Twist)
	pub2 = rospy.Publisher('move', move1D)
	rospy.init_node('move2vel')
	
	rospy.Subscriber("/odom", Odometry, pose_sub)
        
	while not rospy.is_shutdown():
		
		while not moving:
			comm = get_comm()#get command: (r)ight, (l)eft, (i)nit, (q)uit
	
		delta = target-pose
		if abs(delta) > 0.1:
			rospy.loginfo("I'm moving...")
			moving = True
			vel.linear.x = 5*delta
		else:
			rospy.loginfo("done!!")
			vel.linear.x = 0
			moving = False
			pub2.publish(move)
		
		rospy.loginfo("vel = %s, target= %s, pose=%s, ind = %s",vel.linear.x,target,pose,int_pose)
	
		if moving:						
			pub.publish(vel)
			
		rospy.sleep(0.2)
		
def pose_sub(data):
	global pose
	global int_pose
	
	pose = data.pose.pose.position.x
	int_pose = round(pose)
	#rospy.loginfo("my new Pose %s -> %s",pose, int_pose)
	

def get_comm():

	global target
	global moving
	global int_pose
	global move
	
	key = raw_input('type comm: ')
	
	#return comm
	if key == 'r':
		if (int_pose != 7):
			target += 1.0
			moving = True
			move.move = 1
		else:
			print "I can't move... I'm on the limit"
	elif key == 'l':
		if (int_pose != 0):
			target -= 1.0
			moving = True
			move.move = -1
		else:
			print "I can't move... I'm on the limit"
	elif key == 'i':
   		target = 0.0
   		moving = True
   	elif key == 'q':
   		exit()
	elif key == 'h':
   		print "r: move right"
   		print "l: move left"
   		print "i: move to init"
   		print "h: show help"   		
   		print "q: quit"   		   		
   	else:
   		pass
       	
        
if __name__ == '__main__':
    try:
        vel_pub()
        
    except rospy.ROSInterruptException:
    	pass

    
